/* kraut.c */

/*-----------------------------------------------------------------------------
    Hardware configuration:
 -----------------------------------------------------------------------------*/
/*
 * Port 1: Unused
 * Port 2: Unused
 * Port 3: Color sensor
 * Port 4: Unused
 *
 * Port A: Steering motor
 * Port B: Driving motor back left
 * Port C: Driving motor back right
 */

#include <stdio.h>
#include <stdlib.h>
#include <limits.h>
#include <string.h>

#include "kernel.h"
#include "kernel_id.h" 
#include "ecrobot_interface.h"

#include "Car.h"

#define PASS_KEY    "1234"  /* Bluetooth pass key */

/**
 * Hook called before the main program starts. Waits for a BT connection.
 */
void ecrobot_device_initialize()
{
	ecrobot_init_bt_slave(PASS_KEY);
}

/**
 * Hook called after the program shut down.
 */
void ecrobot_device_terminate()
{
	ecrobot_term_bt_connection();
}

/* nxtOSEK hook to be invoked from an ISR in category 2 */
void user_1ms_isr_type2(void){ /* do nothing */ }

/**
 * Command dispatching function. Analyzes @a data and calls the corresponding
 * handler function.
 *
 * @param data The command data
 * @param len Length of the data buffer
 */
static void process_cmd(S8 *data, int len);

/**
 * Initialize steering so that we know min and max values
 */
static void calibrate_steering();

/**
 *
 */
typedef enum
{
   WaitingForStartByte,
   InsideMessage
} ReaderState;

/**
 * The "main" task
 */
TASK(OSEK_Task_Background)
{
	U32 rx_len = 0;
	U32 numDataBytesRead = 0;
	S8 readBuffer[KRAUT_MSG_DATA_LEN];
	ReaderState currentReaderState = WaitingForStartByte;
	S8 byteRead = 0;
	memset(readBuffer, 0, KRAUT_MSG_DATA_LEN);

	calibrate_steering();
	while(1)
	{
		rx_len = ecrobot_read_bt(&byteRead, 0, 1);
		if (rx_len > 0)
		{
         switch(currentReaderState)
         {
         case WaitingForStartByte:
            if ( KRAUT_MSG_START_BYTE == byteRead )
            {
               memset(readBuffer, 0, KRAUT_MSG_DATA_LEN);
               numDataBytesRead = 0;
               currentReaderState = InsideMessage;
            }
            break;
         case InsideMessage:
            if ( KRAUT_MSG_END_BYTE == byteRead )
            {
               // Execute Command
               process_cmd(readBuffer, numDataBytesRead);
               currentReaderState = WaitingForStartByte;
            }
            else if ( KRAUT_MSG_START_BYTE == byteRead )
            {
               memset(readBuffer, 0, KRAUT_MSG_DATA_LEN);
               numDataBytesRead = 0;
            }
            else
            {
               readBuffer[numDataBytesRead] = byteRead;
               numDataBytesRead++;
               if ( numDataBytesRead > KRAUT_MSG_DATA_LEN )
               {
                  currentReaderState = WaitingForStartByte;
               }
            }
            break;
         }
		}
	}
}

/*----------------------------------------------------------------------------*/
static void set_torque(S8 torque)
/*----------------------------------------------------------------------------*/
{
   ecrobot_set_motor_speed(NXT_PORT_B, -torque);
   ecrobot_set_motor_speed(NXT_PORT_C, -torque);
}

int sSteeringMin = 0;
int sSteeringMax = 0;
int currentSteeringPos = 0;

/*----------------------------------------------------------------------------*/
static void set_steering_angle(S8 angle)
/*----------------------------------------------------------------------------*/
{
   // cut values > 100 or < -100
   if ( angle >  100 ) angle =  100;
   if ( angle < -100 ) angle = -100;

   // translate -100..100 request angle to actual steering range:
   int targetPosition = angle;
   targetPosition += 100;
   targetPosition = (int)((((float)sSteeringMax) / 200) * targetPosition);

   display_goto_xy(0,4);
   display_int(targetPosition,4);
   display_update();

   //nxt_motor_set_count(NXT_PORT_A, 0);
   if ( targetPosition == currentSteeringPos )
   {
      return;
   }
   ecrobot_set_motor_speed(NXT_PORT_A, currentSteeringPos < targetPosition ? 10 : -10);
   currentSteeringPos = nxt_motor_get_count(NXT_PORT_A);
   int stickCounter = 0;
   while(currentSteeringPos!=targetPosition)
   {
      int newPos = currentSteeringPos;
      display_goto_xy(0,3);
      display_int(newPos,4);
      display_update();
      systick_wait_ms(10);
      newPos =  nxt_motor_get_count(NXT_PORT_A);
      if (currentSteeringPos == newPos)
      {
         stickCounter++;
         if (stickCounter > 20 ) break;
      }
      else
      {
         stickCounter = 0;
      }
      currentSteeringPos = newPos;
   }
   ecrobot_set_motor_speed(NXT_PORT_A, 0);
   // update one last time after stopping the motor
   currentSteeringPos = nxt_motor_get_count(NXT_PORT_A);
}

/*----------------------------------------------------------------------------*/
static void calibrate_steering()
/*----------------------------------------------------------------------------*/
{
   nxt_motor_set_count(NXT_PORT_A, 0);
   ecrobot_set_motor_speed(NXT_PORT_A, -10);
   int countPrev = nxt_motor_get_count(NXT_PORT_A);
   currentSteeringPos = countPrev;
   do
   {
      display_goto_xy(0,3);
      display_int(currentSteeringPos,4);
      display_update();
      systick_wait_ms(200);
      countPrev = currentSteeringPos;
      currentSteeringPos = nxt_motor_get_count(NXT_PORT_A);
   }
   while(countPrev != currentSteeringPos);

   ecrobot_set_motor_speed(NXT_PORT_A, 0);

   // leftmost position is 0:
   sSteeringMin = 0;
   currentSteeringPos = 0;
   nxt_motor_set_count(NXT_PORT_A, 0);

   ecrobot_set_motor_speed(NXT_PORT_A, 10);

   countPrev = nxt_motor_get_count(NXT_PORT_A);
   currentSteeringPos = countPrev;
   do
   {
      display_goto_xy(0,1);
      display_int(sSteeringMin,4);
      display_goto_xy(0,2);
      display_int(sSteeringMax,4);
      display_goto_xy(0,3);
      display_int(currentSteeringPos,4);
      display_update();
      systick_wait_ms(200);
      countPrev = currentSteeringPos;
      currentSteeringPos = nxt_motor_get_count(NXT_PORT_A);
   }
   while(countPrev != currentSteeringPos);
   ecrobot_set_motor_speed(NXT_PORT_A, 0);
   sSteeringMax = currentSteeringPos;

   display_goto_xy(0,1);
   display_int(sSteeringMin,4);
   display_goto_xy(0,2);
   display_int(sSteeringMax,4);
   display_goto_xy(0,3);
   display_int(currentSteeringPos,4);
   display_update();

   set_steering_angle(0);
}

/*----------------------------------------------------------------------------*/
static void process_cmd(S8 *data, int len)
/*----------------------------------------------------------------------------*/
{
   if(len != KRAUT_MSG_DATA_LEN) return; //nothing to do here

   char cmd = data[0];

   display_clear(0);
   display_goto_xy(0,0);
   display_int(cmd,4);

   display_goto_xy(0,1);
   display_int(data[1],4);

   display_goto_xy(0,2);
   display_int(len,4);

   display_update();

   switch (cmd) {
      case KRAUT_CMD_REQ_TRQ:
         set_torque((S8)data[1]);
         break;

      case KRAUT_CMD_REQ_STEER:
         set_steering_angle((S8)data[1]);
         break;

      default:
         //echo_lcd(data,len);
         //ecrobot_send_bt(data, 0, len);
         break;
   }
}
